#include "CPrimTore.h"
#include <math.h>
#include "polynomial.h"

CPrimTore::CPrimTore(float bigRadius, float smallRadius):IPrimitive(), m_bigRadius(bigRadius), 
		m_smallRadius(smallRadius){}

CPrimTore::~CPrimTore(){}

bool CPrimTore::intersect(const CRay& ray, float& dist, bool useGlobalCoordinates)
{
	//change the coordinate system
	CRay R;
	if (useGlobalCoordinates)
		globalToLocal(ray, R);
	else
		convertRayToLocalCoordinates(ray, R);

	float alpha = R.getDirection().dot(R.getDirection());
	float beta = 2 * R.getDirection().dot(R.getOrigin());
	float gamma = R.getOrigin().dot(R.getOrigin()) - m_smallRadius * m_smallRadius - m_bigRadius * m_bigRadius;

	double coef[5];
	coef[4] = alpha * alpha;
	coef[3] = 2 * alpha * beta;
	coef[2] = beta * beta + 2 * alpha * gamma + 4 * m_bigRadius * m_bigRadius * R.getDirection().z * 
		R.getDirection().z;
	coef[1] = 2 * beta * gamma + 8 * m_bigRadius * m_bigRadius * R.getOrigin().z * R.getDirection().z;
	coef[0] = gamma * gamma + 4 * m_bigRadius * m_bigRadius * R.getOrigin().z * R.getOrigin().z - 
		4 * m_bigRadius * m_bigRadius * m_smallRadius * m_smallRadius;

	//create the polynomial
	Polynomial p(coef[4], coef[3], coef[2], coef[1], coef[0]);

	//check the degree and gets it
	p.Check();
	//int n = p.Degree();

	//solve the equation
	double solutions[4] = {0, 0, 0, 0};
	p.SolveQuartic(solutions);

	//get the greatest solution
	double min = -1;//solutions[0];
	for(int i = 0; i < 4; i++)
	{
		if(solutions[i] > min)
			min = solutions[i];
	}

	if(min <= 0)
		return false;

	//get the smallest solution
	for(int i = 0; i < 4; i++)
	{
		if(solutions[i] < min && solutions[i] > 0)
			min = solutions[i];
	}

	if (min <= 0)
		return false;

	//set the distance
	dist = (float)min;
	return true;

}

/* 
To calculate the normal, we use the gradient
Equation : (x^2 + y^2 + z^2 + R^2 - r^2)^2 - 4R^2(x^2 + z^2)
Gradiant : 
			df/dx = 4x(x^2 + y^2 + z^2 + R^2 - r^2) - 8xR^2
			df/dy = 4y(x^2 + y^2 + z^2 + R^2 - r^2)
			df/dz = 4z(x^2 + y^2 + z^2 + R^2 - r^2) - 8zR^2
Simplify by 4:
			df/dx = x(x^2 + y^2 + z^2 + R^2 - r^2) - 2xR^2
			df/dy = y(x^2 + y^2 + z^2 + R^2 - r^2)
			df/dz = z(x^2 + y^2 + z^2 + R^2 - r^2) - 2zR^2
*/
void CPrimTore::computeNormal(const CRay& ray, float dist, CVector3f& normal)const
{
	//change the coordinate system
	CRay R;
	convertRayToLocalCoordinates(ray, R);

	//calculate intersection point
	CVector3f i = R.getOrigin() + (R.getDirection() * dist);
	
	//temp variable to calculate the gradient
	float R2 = m_bigRadius * m_bigRadius;
	float r2 = m_smallRadius * m_smallRadius;
	float n = i.norme2() - R2 - r2;

	//use the gradient
	normal.x = 4 * i.x * n;
	normal.y = 4 * i.y * n; 
	normal.z = (4 * i.z * n) + (8 * R2 * i.z);
	normal.normalize();

	//if inside
	float d = R.getDirection().dot(normal);
	if(d > 0)
		normal = normal * -1;

	//reset to the global coordinate systeme
	convertNormalToGlobalCoordinates(normal, normal);	
}

bool CPrimTore::isInside(const CVector3f& p)const
{
	//convert p to local coordinates
	CVector3f localP;
	convertPointToLocalCoordinates(p, localP);

	double p1 = localP.norme2() + m_bigRadius * m_bigRadius - m_smallRadius * m_smallRadius;
	double p2 = 4 * m_bigRadius * m_bigRadius * (localP.x * localP.x + localP.z * localP.z);

	if(p1 <= p2)
		return true;

	return false;
}
